#include "obs.h"

// 防碰撞参数
double epsilon = 0.5;       // 防碰撞系数
double keppa_colli = 0.8;   // 势场系数
double r_a = 0.2;           // 势场系数
double r_edge = 0.5;        // 势场边界
double L_detect = 0.5;      // 探测范围半径
double L_min = 0.2;         // 障碍物半径，也是objects.yaml中设置的z参数
double K_outer = 0.01;
double K_inner = 0.01;
Eigen::MatrixXd feedback_temp = (Eigen::MatrixXd(2, 4) << 0, 0, 0, 0,
                                                          0, 0, 0, 0).finished();  

double cal_distance_ij(geometry_msgs::PoseStamped p1, geometry_msgs::PoseStamped p2)
{
    return std::sqrt(pow(p1.pose.position.x - p2.pose.position.x, 2) + pow(p1.pose.position.y - p2.pose.position.y, 2));
}

double cal_distance_ik(geometry_msgs::PoseStamped p1, Eigen::MatrixXd p2)
{
    return std::sqrt(pow(p1.pose.position.x - p2(0,0), 2) + pow(p1.pose.position.y - p2(1,0), 2));
}

Eigen::VectorXd cal_distance_vec(geometry_msgs::PoseStamped *position_agent, Eigen::MatrixXd position_obs, int index)
{
    Eigen::VectorXd result(N_drone + N_UGV + N_obs);
    // 0~4 智能体之间
    for(int i = 0; i < N_drone + N_UGV; i++)
        result(i) = cal_distance_ij(position_agent[index], position_agent[i]);
    // 5~9 智能体与障碍物
    for(int i = N_drone + N_UGV; i < N_drone + N_UGV + N_obs; i++)
        result(i) = cal_distance_ik(position_agent[index], position_obs.col(i - N_drone - N_UGV));
    // 输出
    return result;
}

Eigen::VectorXd cal_weight_vec(geometry_msgs::PoseStamped *position_agent, Eigen::MatrixXd position_obs, int index)
{
    Eigen::VectorXd distance = cal_distance_vec(position_agent, position_obs, index);
    Eigen::VectorXd weight(N_drone + N_UGV + N_obs);
    // 0~4 智能体之间
    for(int i = 0; i < N_drone + N_UGV; i++)
    {
        if(distance(i) >= r_edge)
            weight(i) = 0;
        else if(distance(i) == 0)
            weight(i) = 0;
        else
            weight(i) = -pow(keppa_colli, (r_a - distance(i)) / (r_a - r_edge)) * log(keppa_colli) / ((r_a - r_edge) * distance(i));
    }
    for(int i = N_drone + N_UGV; i < N_drone + N_UGV + N_obs; i++)
    {
        if(distance(i) >= L_detect)
            weight(i) = 0;
        else if(distance(i) <= L_min)
            weight(i) = 0;
        else
            weight(i) = 4 * (pow(distance(i), 2) - pow(L_detect, 2)) * (pow(L_detect, 2) - pow(L_min, 2)) / pow(pow(distance(i), 2) - pow(L_min, 2), 3);
    }
    return weight;
}

Eigen::VectorXd avoid_feedback(geometry_msgs::PoseStamped *position_agent, Eigen::MatrixXd position_obs, int index)
{
    Eigen::VectorXd weight = cal_weight_vec(position_agent, position_obs, index);
    Eigen::VectorXd feedback(2);
    feedback << 0, 0; 
    
    for(int i = 0; i < N_drone + N_UGV; i++)
    {
        feedback(0) += K_inner * epsilon * weight(i) * (position_agent[index].pose.position.x - position_agent[i].pose.position.x);
        feedback(1) += K_inner * epsilon * weight(i) * (position_agent[index].pose.position.y - position_agent[i].pose.position.y);
    }

    for(int i = N_drone + N_UGV; i < N_drone + N_UGV + N_obs; i++)
    {
        feedback(0) += K_outer * epsilon * weight(i) * (position_agent[index].pose.position.x - position_obs(0, i - N_drone - N_UGV));
        feedback(1) += K_outer * epsilon * weight(i) * (position_agent[index].pose.position.y - position_obs(1, i - N_drone - N_UGV));
    }

    feedback = -feedback;

    return feedback;
}